#include "control.h"


double Control::rad2ang(double r)
{
  return 180*r/PI;
}
 Control::Control()
 {
    default_msg.X=0;
    default_msg.Y=14.5;
    default_msg.Z=8;
    flag_start = false;
    flag_mode2 = false;
 }
void Control::getmsg(analyse_data Nowmsg)
{
    this->msg = Nowmsg;
}
void Control::getserver(WiFiClient& sendClient)
{
    this->Client = &sendClient;
}
//函数功能：发送JSON数据至客户端
//函数参数：无
//函数返回值：void
void Control::wirte2Client()
{
    //add message
    //改变单位为毫米级
    default_msg.X = default_msg.X*10;
    default_msg.Y = default_msg.Y*10;
    default_msg.Z = default_msg.Z*10;
    jsonDoc["X"] = default_msg.X;
    jsonDoc["Y"] = default_msg.Y;
    jsonDoc["Z"] = default_msg.Z;
    // 序列化JSON对象为字符串
    serializeJson(this->jsonDoc,this->jsonOutput);
    size_t bytesSent = this->Client->write(this->jsonOutput.c_str());
    Serial.println(this->jsonOutput);
    Serial.println(bytesSent);
    default_msg.X = default_msg.X/10;
    default_msg.Y = default_msg.Y/10;
    default_msg.Z = default_msg.Z/10;
    // 判断是否成功发送
    if (bytesSent > 0) {
        Serial.println("消息成功发送！");
    } else {
        Serial.println("消息发送失败！");
    }
    this->jsonOutput.clear();
}
//函数功能：判断爪子标志位，控制爪子闭合
//函数参数：无
//函数返回值：void
void Control::controlClaw()
{
    if (msg.Claw)
    {
        // 执行爪子打开

        servo.write(claw_pin,80);
        // ture表示打开
    }
    else
    {
        // 执行爪子关闭
        servo.write(claw_pin,30);
        // false表示关闭
    }
}
//函数功能：业务处理框架
//函数参数：无
//函数返回值：void
void Control::controlClawFlag()
{
    // 判断ClawFlag 是否为true
    if (msg.ClawFlag)
    {
        // 选择模式
        if (strncasecmp(msg.States.c_str(), "till", 4) == 0)//给距离
        {
            if (strncasecmp(msg.Button.c_str(), "XUp", 3) == 0)
            {
                default_msg.X = default_msg.X+msg.DPI;
            }
            else if (strncasecmp(msg.Button.c_str(), "XDown", 5) == 0)
            {
                default_msg.X = default_msg.X-msg.DPI;
            }
            else if (strncasecmp(msg.Button.c_str(), "YUp", 3) == 0)
            {
               default_msg.Y = default_msg.Y+msg.DPI;
            }
            else if (strncasecmp(msg.Button.c_str(), "YDown", 5) == 0)
            {
                default_msg.Y = default_msg.Y-msg.DPI;
            }
            else if (strncasecmp(msg.Button.c_str(), "ZUp", 3) == 0)
            {
                default_msg.Z = default_msg.Z+msg.DPI;
            }
            else if (strncasecmp(msg.Button.c_str(), "ZDown", 5) == 0)
            {
                default_msg.Z = default_msg.Z-msg.DPI;
            }
            //临界值判断
            if(default_msg.Y<=5)
            {
              default_msg.Y = default_msg.Y+msg.DPI;
              return;
            }
            if(default_msg.Z>=16)
            {
              default_msg.Z = default_msg.Z-msg.DPI;
              return;
            }
            location2ang();
            servo.write(base_pin,  base_pos + base_adj,   base_spd,  base_ke);
            servo.write(right_pin, right_pos + right_adj, right_spd, right_ke);
            servo.write(left_pin,  left_pos + left_adj,   left_spd,  left_ke);
            wirte2Client();
            this->flag_start = false;
        }
        else if (strncasecmp(msg.States.c_str(), "einmal", 6) == 0)//按住
        {
            while(msg.Einmalflag)
            {
                if (strncasecmp(msg.Button.c_str(), "XUp", 3) == 0)
                {
                    default_msg.X++;
                }
                else if (strncasecmp(msg.Button.c_str(), "XDown", 5) == 0)
                {
                    default_msg.X--;
                }
                else if (strncasecmp(msg.Button.c_str(), "YUp", 3) == 0)
                {
                     default_msg.Y++;
                }
                else if (strncasecmp(msg.Button.c_str(), "YDown", 5) == 0)
                {
                    default_msg.Y--;
                }
                else if (strncasecmp(msg.Button.c_str(), "ZUp", 3) == 0)
                {
                    default_msg.Z++;
                }
                else if (strncasecmp(msg.Button.c_str(), "ZDown", 5) == 0)
                {
                    default_msg.Z--;
                }
                //移动下x y z坐标的函数
                //moveTo(x, y, z);
                //判断x,y,z是否到达最大点的函数
                location2ang();
                servo.write(base_pin,  base_pos + base_adj,   base_spd,  base_ke);
                servo.write(right_pin, right_pos + right_adj, right_spd, right_ke);
                servo.write(left_pin,  left_pos + left_adj,   left_spd,  left_ke);
                wirte2Client();
            }
        }
        else if (strncasecmp(msg.States.c_str(), "null", 4))//给坐标
        {
            //移动
            default_msg.X = msg.X;
            default_msg.Y = msg.Y;
            default_msg.Z = msg.Z;
            
            location2ang();
            servo.write(base_pin,  base_pos + base_adj,   base_spd,  base_ke);
            servo.write(right_pin, right_pos + right_adj, right_spd, right_ke);
            servo.write(left_pin,  left_pos + left_adj,   left_spd,  left_ke);
            wirte2Client();
            this->flag_start = false;
        }
        /*
        else if (strncasecmp(msg.States.c_str(), "mode2", 5))//mode2
        {
          this->flag_mode2 = true;
          while(flag_mode2)
          {
            //归位
            Serial.println("1111");
            mode2.write(27,150);
            mode2.write(12,90);
            mode2.write(13,90);
            mode2.write(14,90);
            delay(1000);
            //1
            mode2.write(12,90);
            mode2.write(14,120);
            mode2.write(27,70);
            delay(1000);
            //2
            mode2.write(27,150);
            mode2.write(12,90);
            mode2.write(13,90);
            mode2.write(14,70);
            delay(1000);
            //3
            mode2.write(12,90);
            mode2.write(14,120);//长臂
            mode2.write(27,150);
            delay(1000);
          }       
        }*/    
    }
    else
    {
        this->flag_start = false;
        return;
    }
}
void Control::location2ang()
{
  //将坐标转为关节角度
    if (default_msg.X > 0)
    {
      joint1_pos = rad2ang(atan(default_msg.Y/default_msg.X));
    }
    
    if (default_msg.X == 0)
    {
      joint1_pos = 90;
    }

    if (default_msg.X < 0)
    {
      joint1_pos = rad2ang(PI - atan(default_msg.Y/-default_msg.X));
    }
    
    double d = sqrt(default_msg.X*default_msg.X + default_msg.Y*default_msg.Y) - 6.5;
    double a = sqrt(default_msg.Z*default_msg.Z + d*d);
    double theta2 = rad2ang(atan(default_msg.Z/d));
    double theta1 = rad2ang(acos(a/16.0));
    joint2_pos = theta1 + theta2;
    joint3_pos = (90 - theta1)*2;

    //将关节角度转为舵机角度
    base_pos  = joint1_pos;
    right_pos = 180 - joint2_pos;
    left_pos  = joint2_pos + joint3_pos - 90;
    claw_pos  = map(joint4_pos/2, 0, 90, 65, 100);

    Serial.println("");
}
